﻿using System;
using System.Collections.Generic;
using System.Text;
using FIRADriverLibrary;
using Geometry;

namespace PotentialFieldDriver.RobAgentSoccer
{
    abstract class RobAgent : IRobAgent
    {
        protected const double robotDiameter = 0.1;

        protected FIRADriverLibrary.FIRADriver firaDriver;
        protected List<FIRARobotState> robots = new List<FIRARobotState>();
        protected System.Drawing.RectangleF pitchArea;
        protected double goalWidth;
        protected double goalStartY;
        protected double goalEndY;
        protected Point2D target;
        protected Point2D home;

        protected List<RobotController> robotControllers = new List<RobotController>();

        protected GameState gameState = GameState.Off;

        public GameState GameState
        {
            get { return gameState; }
            set { gameState = value; }
        }


        public void Setup(FIRADriverLibrary.FIRADriver firaDriver, System.Drawing.RectangleF pitchArea, double goalWidth)
        {
            this.firaDriver = firaDriver;
            this.pitchArea = pitchArea;
            this.goalWidth = goalWidth;

            goalStartY = pitchArea.Top + pitchArea.Height / 2 - goalWidth / 2;
            goalEndY = goalStartY + goalWidth;

            target = new Point2D(pitchArea.Right, pitchArea.Top + pitchArea.Height / 2);
            home = new Point2D(pitchArea.Left, pitchArea.Top + pitchArea.Height / 2);

        }

        public void addRobot(FIRARobotState robot)
        {
            robots.Add(robot);
            robotControllers.Add(new RobotController(robot));
        }

        public void BasicRefresh()
        {
            if (gameState == GameState.Off)
            {
                foreach (RobotController robotController in robotControllers)
                {
                    robotController.Stop();
                }
            }

            if (gameState == GameState.StartupPositions)
            {

                foreach (RobotController robotController in robotControllers)
                {
                    double distance = robotController.Position.GetDistance(robotController.startupPosition);
                    if (distance > robotDiameter)
                        PotencialDriver.Drive(robotController, robotController.startupPosition.x, robotController.startupPosition.y, distance / PotencialDriver.tinyVelocity);
                    else
                        robotController.Stop();
                }
            }

            if (gameState == GameState.Play)
            {

            }
        }

        public abstract void Refresh();

    }
}
